#include "pilink.h"


void pilink_handler_init(struct pilink_handler_s *handler)
{
    handler->status = GSTATUS_IDLE;
}

int pilink_handler_parse(struct pilink_handler_s *handler,unsigned char ch)
{
    switch(handler->status){
        case GSTATUS_IDLE:
            if(ch == PILINK_HEAD1){
                handler->status = GSTATUS_HEAD;
            }
            break;
        case GSTATUS_HEAD:
            if(ch == PILINK_HEAD2){
                handler->status = GSTATUS_SENDER;
            }else{
                handler->status = GSTATUS_IDLE;
				return -1;
            }
            break;
        case GSTATUS_SENDER:
            handler->sender = ch;
            handler->status = GSTATUS_PORT1;
            break;
         
        case GSTATUS_PORT1:
            handler->port  =  ch;
            handler->port  = (handler->port << 8) & 0xFF00;
            handler->status = GSTATUS_PORT2;
            break;

        case GSTATUS_PORT2:
            handler->port |=  ch;;
            handler->status = GSTATUS_LEN1;
            break;
        case GSTATUS_LEN1:
            handler->length = ch;
            handler->length  = (handler->length << 8) & 0xFF00;
            
            handler->status = GSTATUS_LEN2;
            break;
        case GSTATUS_LEN2:
            handler->length |= ch;
            if(handler->length > PILINK_HANDLER_BUFFER_LENGTH){
                handler->status = GSTATUS_IDLE;
                return -6;
            }
            handler->status = GSTATUS_DATA;
            handler->wptr = 0;
            break;
         case GSTATUS_DATA:
            handler->buf[handler->wptr++] = ch;
            if(handler->wptr > handler->length - 1){
                handler->status = GSTATUS_CHECK;
            }
			if(handler->wptr > (PILINK_HANDLER_BUFFER_LENGTH - 1)){
				handler->status = GSTATUS_IDLE;
				return -2;
			}
            break;
         case GSTATUS_CHECK:
            handler->checksum = ch;
            handler->status = GSTATUS_END1;
            break;
         case GSTATUS_END1:
            if(ch == PILINK_END1){
                handler->status = GSTATUS_END2;
            }else{     
                /* length is enough, but end byte 1 missed*/
                handler->status = GSTATUS_IDLE;
                return -3;
            }
            break;
         case GSTATUS_END2:
            if(ch == PILINK_END2){
                if(handler->checksum == pilink_handler_calc_checksum(handler)){
                    handler->status = GSTATUS_IDLE;
                    return 1;
                }else{
                    handler->status = GSTATUS_IDLE;
                    return -4;
                }
                handler->status = GSTATUS_IDLE;
            }else{
                handler->status = GSTATUS_IDLE;
                return -5;
            }
            break;
        default:
            handler->status = GSTATUS_IDLE;
            break;
    };
    return 0;
}

unsigned char pilink_handler_calc_checksum(struct pilink_handler_s *handler)
{
    int i;
    unsigned int sum;
    
    for(i = 0, sum = 0; i < handler->length; i++){
        sum += handler->buf[i];
    }
    
    return (unsigned char)(sum & 0xFF);
}

unsigned char* pilink_handler_get_data(struct pilink_handler_s *handler)
{
    return handler->buf;
}

int pilink_msg_pack(unsigned char *buffer,unsigned char sender,unsigned short port, unsigned char *dat, unsigned short len)
{ 
    unsigned int sum;
    int counter = 0;
    int i;
    
    buffer[counter++] = PILINK_HEAD1;
    buffer[counter++] = PILINK_HEAD2;
    buffer[counter++] = sender;
    buffer[counter++] = (unsigned char)(port >> 8);
    buffer[counter++] = (unsigned char)(port & 0x00FF);
    buffer[counter++] = (unsigned char)(len >> 8);
    buffer[counter++] = (unsigned char)(len & 0x00FF);
    
    for(i = 0,sum = 0; i < len; i++){
        buffer[counter++] = dat[i];
        sum += dat[i];
    }
    
    buffer[counter++] = (unsigned char)(sum & 0xFF);
    buffer[counter++] = PILINK_END1;
    buffer[counter++] = PILINK_END2;
    
    return counter;
}
